#include <buola/image/format.h>
#include <buola/image/io.h>
#include <buola/image/algorithm/calibration.h>
#include <buola/app/ccmdline.h>
#include <buola/io/cfolder.h>
#include <buola/image/cimagegc.h>

using namespace buola;

int main(int pNArg,char **pArgs)
{
    buola_init(pNArg,pArgs);

    try
    {
        if(cmd_line().ArgCount()!=1)
        {
            msg_info() << "must specify source folder\n";
            return -1;
        }
        io::CURI lURI=io::uri(cmd_line().Arg(0));
        io::CFolder lFolder(lURI);

        std::vector<io::CURI> lFiles=lFolder.GetChildrenURIs();

        int lCount=0;

        std::vector<int> lViewSizes;
        std::vector<double> lImagePoints;
        std::vector<double> lObjectPoints;

        std::vector<double> lObject;
        for(int i=0;i<5;i++)
        {
            for(int j=0;j<6;j++)
            {
                lObject.push_back(j*0.54);
                lObject.push_back(i*0.54);
                lObject.push_back(0);
            }
        }
        
        for(const auto &u : lFiles)
        {
            if(u.Extension()!="ppm") continue;
            auto lImage=img::load<img::CImage_rgb8>(u);
            std::vector<CPoint_d> lCorners;
            bool lFound=img::find_chessboard_corners(lImage,6,5,lCorners,0);
            if(lFound)
            {
                img::CImageGC<img::TFormat_rgb8> lGC(lImage);
                lGC.SetSource(img::CColor(0.3,1.0,0.2));
                for(const auto &p : lCorners)
                {
                    lGC.Path().Arc(p,5);
                    lGC.FillPath();
                }

                lViewSizes.push_back(30);
                lObjectPoints.insert(lObjectPoints.end(),lObject.begin(),lObject.end());
                for(int i=0;i<30;i++)
                {
                    lImagePoints.push_back(lCorners[i].x);
                    lImagePoints.push_back(lCorners[i].y);
                }
                
                save(lImage,io::file("calibration"+to_string(lCount)+".png"));
                msg_info() << u.Name() << "   " << ++lCount << "\n";
                if(lCount==50)
                    break;
            }
        }

        img::CCameraParameters lParams;
        img::calibrate_camera(lViewSizes,lImagePoints,lObjectPoints,{640,480},lParams);
//            CV_CALIB_FIX_PRINCIPAL_POINT|CV_CALIB_ZERO_TANGENT_DIST|CV_CALIB_FIX_K1|CV_CALIB_FIX_K2|CV_CALIB_FIX_K3|
//            CV_CALIB_FIX_K4|CV_CALIB_FIX_K5|CV_CALIB_FIX_K6);
        auto lUndistortSrc=img::load<img::CImage_rgb8>(io::get_home()/"tabletop5.pcd");
        img::CImage_rgb8 lUndistortDst({640,480});
        img::undistort(lUndistortSrc,lUndistortDst,lParams);
        img::save(lUndistortDst,io::get_home()/"undistort.png");
        msg_info() << lParams << "\n";
    }
    catch(std::exception &pE)
    {
        msg_info() << "caught exception " << pE.what() << "\n";
    }

    return buola_finish();
}
